#include <AccelStepper.h>
#include <iostream>
#include <string>



//Intalize Stepper Motor
int motorSpeed = 9600; //maximum steps per second (about 3rps / at 16 microsteps)
int motorAccel = 80000; //steps/second/second to accelerate

int motorDirPin = 2; //digital pin 2
int motorStepPin = 3; //digital pin 3

//set up the accelStepper intance
//the "1" tells it we are using a driver
AccelStepper stepper(1, motorStepPin, motorDirPin); 


//Intallize Cup sensor
int weightSensorPin = 0;    // select the input pin for the Weight Sensor
double weightThreshold = .10;


//pretend this is a serial communication:
String serialComOne[3] = {"49000","1","2"};
String serialComTwo[3] = {"35000","0","1"};


//use this as a global variable to keep track of arduino's current goal.  when arduino receives serial command from the computer, it will be in this string format "1429,1,1"
//in order to convert the string into a usable format, use the "ParseSerial(string) command to update the current goal.  when goal is complete, will set all values to -1.
String CurrentCommands[3];

void setup() {
  //Setup Variables for Program
  CurrentCommands[0] = "-1";  //StationID or location
  CurrentCommands[1] = "-1";  //DrinkType
  CurrentCommands[2] = "-1";        //Qty
  
  //Setup stepper motor  
  stepper.setMaxSpeed(motorSpeed);
  stepper.setSpeed(motorSpeed);
  stepper.setAcceleration(motorAccel);
  
  //Have it move in specific direction until it hits a switch.  at which point this will turn off, and reset the starting point of the stepper motor position
  //stepper.moveTo(32000); //move 32000 steps (should be 10 rev)

  /*
   INPUTS NEEDED TO SELECT DRINK:
   - StationID
   - DrinkType  (effects the use of QTY)
   - QTY
   
   Sample set:
   "49000,1,2"  = "move to station 49000, dispense two oz of mixer (because drinktype 1 = mixer)"
   "35000,0,1"  = "move to station 35000, dispense one shot spirit (becasuse drinktype 2 = spirit)"   
   */


  Serial.begin(9600);

}

void loop() {

  //Receives instructions
  ParseSerial("testing");
  
  
  //this function will postpone everything until there is a cup on the transport.
  CheckIfSensesCup();
  

  

  
  
  

  //if stepper is at desired location
  if (stepper.distanceToGo() == 0){
    //go the other way the same amount of steps
    //so if current position is 400 steps out, go position -400
    stepper.moveTo(-stepper.currentPosition());
    //these must be called as often as possible to ensure smooth operation
    //any delay will cause jerky motion
     
  }
  stepper.run();





  delay(5000);


}

void ParseSerial(String serialData)
{
  String StationID = "15000", DrinkType = "0", Qty = "2";  
  
  //parse the data, and pass the pieces into "CurrentCommands"
  CurrentCommands[0] = StationID;  //StationID or location
  CurrentCommands[1] = DrinkType;  //DrinkType
  CurrentCommands[2] = Qty;        //Qty
  
  //For debugging purposes: print the result
  Serial.print("Station: ");
  Serial.println(CurrentCommands[0]);
  Serial.print("Drink Type: ");
  Serial.println(CurrentCommands[1]);  
  Serial.print("Qty: ");
  Serial.println(CurrentCommands[2]);
}


boolean CheckIfSensesCup()
{
    
    while( weightThreshold > analogRead(weightSensorPin) )
    {
        Serial.println("Waiting for cup...");
        delay(500);           
    }
  
    Serial.println("");
    Serial.println("Cup registered");  
}



/* 

FUTURE USE:
SERIAL COMMUNICATION

  String serialDataIn;
  String data[3];
  int counter;
  int inbyte;
  counter = 0;
  serialDataIn = String("");
  inbyte = 0;
  
  if(Serial.available()){
    inbyte = Serial.read();
    if(inbyte >= '0')
      serialDataIn += inbyte;
    if (inbyte == ','){  // Handle delimiter
      data[counter] = String(serialDataIn);
      serialDataIn = String("");
      Serial.println(data[counter]);
      counter = counter + 1;
    }
    if(inbyte ==  '\r'){  // end of line
      //handle end of line a do something with data
      Serial.println("    - end transmission");
    }        
  }
  
*/
